#include "stdafx.h"
#include "Renderer.h"
#include "OpenCLGL_Util.h"
#include <thread>
#include "CostFunction.h"
#include "ParticleSwarm.h"
#include "HandDetector3D.h"

GLFWwindow * mWindow;

using namespace cv;
using namespace std;

//C:\Users\HoangQC\Desktop\TestCase-1\Result
void printDofs(float * p);

void main()
{
	Mat obsColor = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_1.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_1.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_1.bmp",CV_LOAD_IMAGE_UNCHANGED);

	Mat obsColor2 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_2.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth2 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_2.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor2 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_2.bmp",CV_LOAD_IMAGE_UNCHANGED);

	Mat obsColor3 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_3.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth3 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_3.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor3 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_3.bmp",CV_LOAD_IMAGE_UNCHANGED);

	Mat obsColor4 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_4.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth4 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_4.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor4 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_4.bmp",CV_LOAD_IMAGE_UNCHANGED);

	Mat obsColor5 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_5.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth5 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_5.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor5 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_5.bmp",CV_LOAD_IMAGE_UNCHANGED);

	Mat obsColor6 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_6.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth6 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_6.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor6 = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_6.bmp",CV_LOAD_IMAGE_UNCHANGED);

	/*Point3i(304,266,957); = -28.8,-46.8,957*/
	//310-304
	/*Mat preview = Mat(256,256*6,CV_8UC3);

	rawColor(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(0,0,256,256)));
	rawColor2(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(256,0,256,256)));
	rawColor3(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(512,0,256,256)));
	rawColor4(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(768,0,256,256)));
	rawColor5(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(1024,0,256,256)));
	rawColor6(Rect(304-128,266-128,256,256)).copyTo(preview(Rect(1280,0,256,256)));

	imshow("lll",preview);
	imwrite("ho.bmp",preview);
	waitKey();
	return;*/
	HandDetector3D detector(mWindow);

	detector.setObservation(obsDepth,obsColor,Vec3d(-30.f, -46.8f, 954),true);
	detector.trackingFirstFrame();
	cv::Mat image(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K",rawColor);
	waitKey();

	/*NEXT STEPS*/
	detector.setObservation(obsDepth2,obsColor2,Vec3d(-30.f, -46.8f, 954),false);
	detector.trackingNextFrame();
	image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor2,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K.2",rawColor2);
	cv::waitKey();
	/*NEXT STEPS---*/
	detector.setObservation(obsDepth3,obsColor3,Vec3d(-30.f, -46.8f, 954),false);
	detector.trackingNextFrame();
	image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor3,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K.3",rawColor3);
	cv::waitKey();
	/*NEXT STEPS---*/
	detector.setObservation(obsDepth4,obsColor4,Vec3d(-30.f, -46.8f, 954),false);
	detector.trackingNextFrame();
	image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor4,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K.4",rawColor4);
	cv::waitKey();
	/*NEXT STEPS---*/
	detector.setObservation(obsDepth5,obsColor5,Vec3d(-30.f, -46.8f, 954),false);
	detector.trackingNextFrame();
	image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor5,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K.5",rawColor5);
	cv::waitKey();
	/*NEXT STEPS---*/
	detector.setObservation(obsDepth6,obsColor6,Vec3d(-30.f, -46.8f, 954),false);
	detector.trackingNextFrame();
	image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	detector.drawResultToMat(rawColor6,cv::Rect(304-64,266-64,128,128));
	cv::imshow("F.U.K.6",rawColor6);
	cv::waitKey();

	imwrite("result1.bmp",rawColor);
	imwrite("result2.bmp",rawColor2);
	imwrite("result3.bmp",rawColor3);
	imwrite("result4.bmp",rawColor4);
	imwrite("result5.bmp",rawColor5);
	imwrite("result6.bmp",rawColor6);
}

void printDofs(float * p)
{
	printf("WRIST:\n %.1f; %.1f; %.1f\n %.1f; %.1f; %.1f\n",p[0],p[1],p[2],p[3],p[4],p[5]);
	printf("THUMB: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_THUMB],p[OFFSET_THUMB+1],p[OFFSET_THUMB+2],p[OFFSET_THUMB+3]);
	printf("FING1: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER1],p[OFFSET_FINGER1+1],p[OFFSET_FINGER1+2],p[OFFSET_FINGER1+3]);
	printf("FING2: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER2],p[OFFSET_FINGER2+1],p[OFFSET_FINGER2+2],p[OFFSET_FINGER2+3]);
	printf("FING3: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER3],p[OFFSET_FINGER3+1],p[OFFSET_FINGER3+2],p[OFFSET_FINGER3+3]);
	printf("LITTLE: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_LITTLE],p[OFFSET_LITTLE+1],p[OFFSET_LITTLE+2],p[OFFSET_LITTLE+3]);
}